Motion 範例

此範例程式說明如何透過 Win32 應用程式使用 KINGSTAR 子系統及介紹 API 函式。在此範例中，Win32 指的是 Windows API，而非 32 位元，KINGSTAR Win32 API 與 KINGSTAR RT API 相同，使用 Win32 介面時，不需了解 RTX64 和即時開發，因開啟與停止子系統將會自動開啟和停止即時環境。

因即時和非即時之間的連接為異步的，且 Windows 非即時環境，所有對子系統的呼叫都將佇列， 因此應用程式無法有非常短 (<5 ms) 的更新週期，若想要更快的更新週期，請考慮使用 RT 介面代替。

編譯及執行範例程式

範例檔案位於 C:\Users\Public\Public Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\Motion_Sample，打開 Motion_Sample.sln 並編譯之。

注意：檔案總管 (File Explorer) 有兩個路徑：階層路徑與完整路徑，階層路徑顯示在地址欄中；完整路徑顯示在檔案總管上方。右鍵點擊 Motion_Sample.sln 後點選 Properties，將可看見位置 (Location) 為 C:\Users\Public\Documents\IntervalZero\KINGSTAR SDK\<Version Number>\Samples\Motion_Sample，此即為完整路徑；而注意階層路徑為 Public Documents。若使用非英文的 Windows 系統，而需要複製貼上路徑至地址欄中以加快查找範例速度，則請務必使用完整路徑；若想要透過點擊瀏覽範例檔案夾，則請使用階層路徑；英文版 Windows 之檔案夾將自動重新導向，因此就算貼上階層路徑，檔案總管亦可引導至範例程式。

下圖為範例程式的輸出：

在 Visual Studio 中設定專案 properties

此範例為使用 Visual Studio 2019 中的 C++ 與 MFC 64 位元所開發之 MFC 應用程式，開發應用程式時，只要此應用程式為 64 位元，即可自行選擇開發環境，因控制即時子系統需使用 64 位元。更多關於 Motion 範例的使用者介面之資訊，請見其 使用者介面。

創建應用程式時需在 Visual Studio 內修改以下屬性：

1. 在 Solution Explorer 中的專案名稱上點擊右鍵，接著點選 Properties。
2. 進入 (Project name) Property Pages 對話框中的左窗格，點開 C/C++ 清單並點選 General。
3. 在右方區域的 Additional Include Directories 方框輸入 "$(RTX64SDKDir4)include;$(KINGSTARSDKDir4)include;%(AdditionalIncludeDirectories)" （無空白鑑）。
4. 在左窗格中點開 Linker 清單後點選 General。
5. 右側 Additional Library Directories 方框中，輸入"$(KINGSTARSDKDir4)lib\amd64\;%(AdditionalLibraryDirectories)"（無空白鍵）。
6. 在左窗格中的 Linker 清單中，點選 Input。
7. 在右方區域的 Additional Dependencies 方框輸入 "KsApi.lib;%(AdditionalDependencies)" （無空白鑑）。
8. 點擊 Apply 後再按一下 OK。
9. 在專案的標頭檔（名稱為 ProjectName.h）之 #endif // UNDER_RTSS 底下，輸入以下代碼：

#include <ksapi.h>
#include <ksmotion.h>

配置 KINGSTAR 子系統

使用 OnBnClickedInit 函式以開啟 KINGSTAR 子系統，此函式在點擊 Main menu 區域的 Init 時呼叫，其透過 GetSafeHwnd 函式（啟動 window 的控制代碼）獲取參數 m_hWnd，之後將使用 _beginthread 函式建立一個新的執行緒，該執行緒為 InitThread 位址發送處，此 InitThread 函式負責所有初始化 KINGSTAR 子系統的工作，包括設定參數、功能、軸與 I/O 模組及連結。

範例使用的以下變數，其皆在 Motion_TestDlg.cpp 的開始即宣告。

• KsCommandStatus Command = { 0 } – KsCommandStatus 為包含函式狀態的結構，可得知函式是否正在執行、完成、回傳任何錯誤及何錯誤，我們使用 Command 以獲取函式狀態。
• KsError Code = errNoError – KsError 為包含 KINGSTAR 所有錯誤碼的列舉類型，errNoError 表示函式已成功。我們使用 Code 以接收函式的回傳值，errNoError 表示函式已成功執行；若 Code 非 errNoError，則將呼叫 Destroy 而 KINGSTAR 子系統將終止。
• 旗標：通知程式條件已達到。
  • LinkInit – 訊號表示 EtherCAT 連結是否已建立。
  • LinkStart – 訊號表示 KINGSTAR 子系統是否已開啟。
  • BThreadFlag – 訊號表示 KINGSTAR 處理序是否開始。
  • BCloseFlag – 訊號表示 KINGSTAR 處理序是否已被終結。
  • Edit_Flag – 訊號表示文字方塊中的值已變更。
  • UIInit_Flag – 訊號表示在 EtherCAT 狀態設為 Op 後，使用者介面使否準備好初始化。

初始化 KINGSTAR 子系統之連結

首先呼叫 Create，準備連接應用程式至 KINGSTAR 子系統，開始任何動作前，必須最先呼叫 Create。

Code = ::Create(0, 0);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to Create EtherCAT: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   return;
}
BThreadFlag = true;

設定 EtherCAT 循環時間

SetCycleTime設定 EtherCAT 循環時間，其時間單位為秒。欲使用低於 1 毫秒的循環時間，須備有高速計時器套件。注意非所有軸皆支援快速循環時間，若選擇了不支援的循環時間，則每個軸的更新時間會自動且各自延長，欲使用快速循環時間，請確保電腦上的網卡可使用，只有具有低延遲的網卡才可支持快速循環。

Code = ::SetCycleTime(cycle1000);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to set SetCycleTime: %d\n"), Command.ErrorId);	
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

停用 RTX64 伺服主控台 (Server Console) 上的記錄

EnableServerLog啟用或停用 RTX64 Server Console 上的即時訊息，若將其停用，則主控台將只顯示 KINGSTAR 訊息，因此我們選擇將其啟用。

Code = ::EnableServerLog(true);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableServerLog: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

設定存取模式

SetAxisAccessMode設定 EtherCAT 驅動器之資料傳送模式，存取模式決定驅動器可用的控制模式，存取模式可在 KsAccessMode 列舉類型中選擇，預設之存取模式為 accessVelPos，

Code = ::SetAxisAccessMode(accessVelPos);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to SetAxisAccessMode: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

啟用軸的數位輸入及輸出

EnableAxisInput和 EnableAxisOutput 啟用或停用存取軸的數位輸出及輸入，首三個輸入位元為負超程 (Overtravel)、正超程與原點復歸感測器，若輸入啟用後可使用超程位元。

Code = ::EnableAxisInput(TRUE);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableAxisInput: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

Code = ::EnableAxisOutput(TRUE);
if (Code != errNoError) {
   Str_Error.Format(_T("Failed to EnableAxisOutput: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);
   ::Destroy();
   return;
}

設定模擬軸的數量

SetConfiguredAxesCount設定模擬軸的數量，我們將數量設為一，若沒有真實軸，將為 EtherCAT 網路建立一個模擬軸。

::SetConfiguredAxesCount(1);

啟用實際速度

EnableActualVelocity讀取軸的實際速度。

::EnableActualVelocity(true);

Motion 參數、單位轉換及模擬 I/O 模組

在將範例連接到 KINGSTAR 子系統之前，我們使用以下函式來設定軸和 I/O 模組的運動參數和轉換比率。

SetAxisMotionProfile：設定軸的運動軌跡。在此範例中，運動軌跡定義在 McProfileSettings 結構之 ProfileSettings 的實例中。

SetAxisCountsPerUnit：將使用者自定義位置單位的轉換比率設定為軸使用的計數（脈衝）單位。在此範例中，Numerator 和 Denominator 為一，因此比率為 1:1，Reverse 為 false，因此軸的方向未倒轉。

EnableAxisUnitConversion：啟用軸使用真實世界單位。使用 SetAxisCountsPerUnit 設定轉換比率後，需使用此函式來起動轉換，此比率才會生效。

ConfigureIo：配置模擬 I/O 模組之設定。

SubsystemStatus：此結構用以獲取 EtherCAT 連結的狀態。我們宣告 SubsystemStatus KSMStatus 之實例，並使用 AxesCount 欄位獲取 EtherCAT 網路上軸的數量。

SlaveStatus：此結構用來配置模擬 I/O 模組之設定。InputLength 與 OutputLength 為必填欄位，其決定了模擬 I/O 模組之輸入及輸出數量，其他於 SlaveStatus 中的欄位可留空，將會由預設值填入；若使用真實 I/O 模組，KINGSTAR 將自動偵測其輸入與輸出並填入所有欄位，我們宣告 SlaveStatus IoModuleStatus 之實例，以配置設定。

//Set up axes.
for (int i = 0; i < KSMStatus.AxesCount; i++)
{
   ::SetAxisMotionProfile(i, profileUnitPerSecond, ProfileSettings);
   ::SetAxisCountsPerUnit(i, Numerator, Denominator, Reverse);
   ::EnableAxisUnitConversion(i, TRUE);
}

//Set up I/O modules.
for (int i = 0; i < IOCOUNT; i++)
{
   SlaveStatus IoModuleStatus = { 0 };
   IoModuleStatus.InputLength = 16;
   IoModuleStatus.OutputLength = 16;
   ::ConfigureIo(i, IoModuleStatus);
}

開啟 KINGSTAR 子系統

我們使用 Start 來開啟 KINGSTAR 子系統和 EtherCAT 網路，當子系統無法啟動造成程序永遠在等待完成，我們為 Start 使用 WaitForCommand 將超時設置為 30 秒；若子系統開啟，旗標 LinkInit 及 LinkStart 將設為 true；若 KINGSTAR 子系統無法啟動，Start 將中止而 LinkInit 將設為 false。

在所有配置完成且 KINGSTAR 子系統開啟後，UIInit_Flag 將設為 true。

Command = WaitForCommand(30, TRUE,::Start());
if (Command.Done)
{
   LinkInit = LinkStart = true;
}
else
{
   Str_Error.Format(_T("Failed to Start: %d\n"), Command.ErrorId);
   ::PostMessageA(hWnd, WM_UPDATE_ERROR, 0, 0);

   LinkInit = false;
   return;
}
UIInit_Flag = true;

使用 KINGSTAR 子系統。

將真實裝置連接到 KINGSTAR 子系統後，幾個區域會顯示裝置的信息：MotionStatus、MasterStatus、SlaveStatus、ServoStatus、IOStatus，資訊將會依裝置有所不同，可使用 Slave ID 以選擇想要的裝置，若無真實裝置，則這些區域的大多數欄位將會是零。

軸

OnBnClickedServoOn 和 OnBnClickedServoOff 函式各別啟用和停用軸。

Servo on

啟用軸之前須檢查一些狀態，第一個為 EtherCAT 連接狀態，若狀態為 ecatOP，檢查 EtherCAT 連結旗標 LinkStart，若為 true 則準備啟用軸，啟用之前使用 ResetAxis 來重置警報以防其中有錯誤，接著檢查 SERVOCOUNT，若其大於零，則檢查軸的 StatusWord 物件，第三位元（位元 2）應為 "操作啟用 (operation enabled)"，若此位元為 true 則軸已啟用。欲知第三位元是否為 true，使用位元 AND 運算子 (&)，條件 "if (ServoNoFlag(KSStatusWord[i]) != 1)" 如下描述：

1. 對選定軸的值和十六進制值 0x4 執行邏輯 AND 運算。
2. 取 AND 運算的結果並將值的位元向右移動兩個位置。
3. 移動位元後，檢查其是否等於一 (1)，若不是一，請執行以下代碼。

若結果不等於一，代表軸未啟動，因此我們使用 PowerAxis 來啟動軸，在 PowerAxis 中，啟動 (Enable) 參數為 TRUE。

void CMotion_TestDlg::OnBnClickedServoOn()
{
   if(KSMStatus.State == ecatOP)
   {
      if(LinkStart)
      {
         for(int i=0; i < KSMStatus.AxesCount; i++)
	     {	
	        ::ResetAxis(i);

            #if SERVOCOUNT > 0
	        if (ServoNoFlag(KSStatusWord[i]) != 1)
	        {
	           ::PowerAxis(i, TRUE, TRUE, TRUE);
	        }	
	        #endif
         }
      }	
   }
   else
   {
      MessageBox(_T("KSMStatus->EcatState != ecatOP"));	
   }
}

Servo off

欲停用軸，使用 PowerAxis，Enable 為 FALSE。

void CMotion_TestDlg::OnBnClickedServoOff()
{
   for (int i = 0; i < KSMStatus.AxesCount; i++)
      ::PowerAxis(i, FALSE, TRUE, TRUE);
}

重設警報

OnBnClickedResetalarm 函式重設警報，軸執行時可能會有錯誤發生，當錯誤發生時警報即出現，因此我們使用 ResetAxis 來重設軸的警報。

void CMotion_TestDlg::OnBnClickedResetalarm()
{
   for (int i = 0; i < KSMStatus.AxesCount; i++)
   {
      ::ResetAxis(i);
   }
}

Motion 控制

Jog 和 Move 為測試軸的基本運動，Jog 在不指定目標位置情況下測試速度；Move 在有目標位置情況下測試移動，而 Home 使軸回歸原點。

寸動 (Jog)

Jog 在給定的方向使用 T 曲線加速度，變數 UJog_Pulse 儲存寸動之速度，其預設值為 10000，m_AxisNumber.GetCurSel() 獲取軸之索引，JogAxis 執行寸動運動，移動方向可為向前或向後，+JOG 使用函式 OnBnClickedPjog 以向前移動；而 -JOG 使用 OnBnClickedNjog 以向後移動。

void CMotion_TestDlg::OnBnClickedPjog()
{
   ::JogAxis(m_AxisNumber.GetCurSel(), double(UJog_Pulse), 36000, 36000, 360000, mcPositiveDirection);
}
void CMotion_TestDlg::OnBnClickedNjog()
{
   ::JogAxis(m_AxisNumber.GetCurSel(), double(UJog_Pulse), 36000, 36000, 360000, mcNegativeDirection);
}

使用 HaltAxis 以停止寸動運動。

void CMotion_TestDlg::OnBnClickedJogstop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

當輸入寸動速度的新值，UJog_Pulse 將更新，UpdateData 在 UI 上讀取更新的速度，資料更改後，旗標 Edit_Flag 將設為 true。

void CMotion_TestDlg::OnEnChangeJogPulse()
{
   Edit_Flag = false;
   UpdateData(true);
   Edit_Flag = true;
}

移動 (Move)

Move 使用運動曲線以建立 S 曲線軌跡至目的地，與 Jog 相似，變數 UMove_Pulse 儲存移動的距離，其預設值為 10000，m_AxisNumber.GetCurSel() 獲取軸之索引。其中有兩個 MoveAxis 函式：MoveAxisAbsolute 與 MoveAxisRelative，此範例使用 MoveAxisRelative，移動方向可為向前或向後，+MOVE 使用函式 OnBnClickedPmove 以向前移動；而 -MOVE 使用 OnBnClickedNmove 以向後移動。

void CMotion_TestDlg::OnBnClickedPmove()
{
   ::MoveAxisRelative(m_AxisNumber.GetCurSel(), double(UMove_Pulse), 3600, 36000, 36000, 3600000, mcAborting);
}
void CMotion_TestDlg::OnBnClickedNmove()
{
   ::MoveAxisRelative(m_AxisNumber.GetCurSel(), double(0-UMove_Pulse), 3600, 36000, 36000, 3600000, mcAborting);
}

使用 HaltAxis 以停止移動。

void CMotion_TestDlg::OnBnClickedMovestop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

當輸入 MoveAxisRelative 之距離的新值，UJog_Pulse 將更新，UpdateData 在 UI 上讀取更新的距離，資料更改後，旗標 Edit_Flag 將設為 true。

void CMotion_TestDlg::OnEnChangeMovePulse()
{
   Edit_Flag = false;
   UpdateData(true);
   Edit_Flag = true;
}

原點復歸 (Home)

Home 將軸移動到已知位置，其中原點復歸過程寫在函式 OnBnClickedHome 中。欲將軸復歸原點，先使用 SetAxisParameter 以設定原點復歸模式，在此範例使用 mcSlaveHomingMode，其為伺服驅動器所提供的原點復歸方法，接著，使用 HomeAxis 以將軸回歸原點，而 m_AxisNumber.GetCurSel() 獲取軸之索引。

void CMotion_TestDlg::OnBnClickedHome()
{
   //There is test the slave homing.
   ::SetAxisParameter(m_AxisNumber.GetCurSel(), mcSlaveHomingMode, 33, mcImmediately);
   Command = WaitForCommand(30, TRUE, ::HomeAxis(m_AxisNumber.GetCurSel(), 0, HomeVel, 
           HomeEndVel, HomeAcc, HomeDec, HomeJerk, mcPositiveDirection, homingSlave));
   if (!Command.Done)
   {
      Str_Error.Format(_T("Failed to Home: %d\n"), Command.ErrorId);
      GetDlgItem(IDC_ERROR_RETURN)->SetWindowText(Str_Error);
   }
}

使用 HaltAxis 以停用原點復歸。

void CMotion_TestDlg::OnBnClickedHomestop()
{
   ::HaltAxis(m_AxisNumber.GetCurSel(), 3600, 360000, mcAborting);
}

獲取狀態

當硬體連接時，我們可以透過某些變數和函式來檢查它的狀態。

索引

在此範例中宣告了五個變數來接收裝置的索引：

• m_AxisNumber：軸的索引（伺服馬達），其在運動控制 (Motion control) 區域中用作伺服索引 (Servo Index)，請見 使用者介面。
• m_ServoNumber：軸的索引，其在伺服器狀態 (ServoStatus) 區域中用作伺服索引 (Servo Index)。
• m_MotionNumber：軸的索引，其在運動狀態 (MotionStatus) 區域中用作伺服索引 (Servo Index)。
• m_SlaveNumber： EtherCAT 從站的索引，其在從站狀態 (SlaveStatus ) 區域中用作從站 ID (Slave ID)。
• m_IONumber： I/O 模組的索引，其在 IOStatus 區域中用作 I/O 索引 (I/O Index)。

EtherCAT 主站之狀態

使用 GetStatus 以獲取 EtherCAT 主站狀態。

GetStatus(&KSMStatus, KSSubsystemDiagnostics);

EtherCAT 從站之狀態

使用 GetSlaveById 以獲取 EtherCAT 從站狀態。

::GetSlaveById(i, &KSSlaveStatus[i]);

軸狀態

使用 GetAxisMotionState 以獲取軸（伺服驅動器）之運動狀態。

在 SlaveStatus 與 ServoStatus 區域中，使用 GetAxisByIndex 以獲取軸的資訊與編碼器解析度。

在 ServoStatus 區域中，使用 ReadAxisStatusWord 以獲取軸之 CANopen (CiA 402) 狀態（從 StatusWord 至 LimitActive）。

在 ServoStatus 區域中，使用函式： GetAxisVelocity, GetAxisPosition, ReadAxisInputs, ReadAxisOutputs 以獲取 Velocity, Position, Servo DI, Servo DO 之狀態。

在 ServoStatus 區域中，使用變數 Read_HomingStatus 以獲取軸的原點復歸狀態 (Homing Status)。一般來說，伺服驅動器的數位輸入之前三個位元（位元 0, 1, 2） 為負超程 (Negative Overtravel)、正超程 (Positive Overtravel) 及原點感測器 (Home Sensor)，但位元可能因伺服驅動器而有所不同，在此範例中我們檢查位元 0 或位元 1 和位元 2 是否為 TRUE （位元 1 和 2 一起檢查），並將結果分配給 Read_HomingStatus。

while(BCloseFlag)
{
   while (BThreadFlag)
   {
      ::GetStatus(&KSMStatus, KSSubsystemDiagnostics);
      for(int i = 0; i < KSMStatus.AxesCount; i++)
      {
         ::GetAxisMotionState(i, MotionCXY[i].KSMcMotionState, MotionCXY[i].KSMcDirection);
         ::GetAxisByIndex(i, &KSAxisStatus[i], &AXIS_RESOLUTION[i], &AXIS_DI[i], &AXIS_DO[i]);
         ::ReadAxisStatusWord(i, &KSStatusWord[i]);   //Read StatusWord 
         ::ReadAxisActualVelocity(i, MotionCXY[i].VelocityValue);   //Get Velocity status
         ::ReadAxisActualPosition(i, MotionCXY[i].PositionValue);   //Get PositionValue status
         SERVOIO_DI_ERROR[i] = ::ReadAxisInputs(i, &SERVOIO_DI[i]);   //Get ServoDI
         SERVOIO_DO_ERROR[i] = ::ReadAxisOutputs(i, &SERVOIO_DO[i]);   //Get ServoDO	
         Read_HomingStatus[i] = HomingStatus(KSStatusWord[i]);
      }
   .........
}

I/O 模組之狀態

使用 ReadInputWord 和 ReadOutputWord 獲取 I/O 模組狀態，IO_DI 與 IO_DO 接收讀取的 WORD 值。

for(int i = 0; i < KSMStatus.IOCount; i++)
{
   #if IOCOUNT > 0
   ::ReadInputWord(i, 0, &IO_DI[i]);
   ::ReadOutputWord(i, 0, &IO_DO[i]);
   #endif
}

停止 KINGSTAR 子系統

在停止 KINGSTAR 子系統前須先停止所有軸，可用 HaltAxis 以停止軸。

KINGSTAR 子系統必須在關閉應用程式前停止，若電腦未預期關閉，所有偵測到主站遺失的 EtherCAT 從站都將被置放到安全警告狀態，子系統會在下一次開啟時嘗試重置從站，然而有些從站在重啟後可能不會正常運作，若此情況發生，請關閉、重啟或重置這些從站，欲重置從站，使用 ResetAxis 函式或重啟從站。

關閉連結

欲確保 KINGSTAR 子系統已停止，添加了關閉邏輯至 OnBnClickedExit 函式，首先設定旗標 BCloseFlag 和 BThreadFlag 為 false，接著使用 Stop 以停止 EtherCAT 連結和使用 Destroy 終止 KINGSTAR 子系統處理序，

void CMotion_TestDlg::OnBnClickedExit()
{
   BCloseFlag = BThreadFlag = false;
   ::Stop();
   ::Destroy();
   CDialogEx::OnOK();
}

若有多個應用程式在使用 KINGSTAR，請記住此任一應用程式在呼叫 Destroy 函式前不可呼叫 KINGSTAR 函式。